#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/laser_scan.hpp>
#include <pcl/point_types.h>
#include <pcl/registration/icp.h>
#include <pcl_conversions/pcl_conversions.h>
#include <tf2_ros/static_transform_broadcaster.h>
#include <tf2_ros/transform_broadcaster.h>
#include <geometry_msgs/msg/transform_stamped.hpp>

#include "publish_pointcloud.hpp"
using namespace std::chrono_literals;


class mult_lidar : public rclcpp::Node {
    public:
        mult_lidar() : Node("mult_lidar") {
            // 订阅 cloud 消息
            // cloud_sub = this->create_subscription<sensor_msgs::msg::PointCloud2>("/cloud", 
            //     10, std::bind(&LaserToTF::cloud_topic_callback, this, std::placeholders::_1));
            // 订阅 LaserScan 消息
            cloud_sub = this->create_subscription<sensor_msgs::msg::PointCloud2>(
                "pointcloud", 10, std::bind(&mult_lidar::cloud_topic_callback, this, std::placeholders::_1));
                pub_cloud = create_publisher<sensor_msgs::msg::PointCloud2>("/convert_cloud", 10);    
           // auto output_msg = std::make_unique<sensor_msgs::msg::PointCloud2>();    
           dsp_cloud.reset(new  pcl::PointCloud<pcl::PointXYZ>);
           target_cloud.reset(new  pcl::PointCloud<pcl::PointXYZ>);
          // pcl::io::loadPCDFile("/home/orangepi/laser_processor/saved_pointloud.pcd",  *target_cloud);
         //  pcl::io::loadPCDFile("/home/orangepi/laser_processor/mult_lidar/ss1.pcd",  *target_cloud);
          // std::size_t size=target_cloud->points.size();
       //   comput3Dcenter(target_cloud,x0,y0);
            // 初始化标志位
           // first_frame_received_ = false;
           count=0;
        }
    private:
        void cloud_topic_callback(const sensor_msgs::msg::PointCloud2::SharedPtr msg)
         {
            // pcl::PointCloud<pcl::PointXYZ> laser_cloud;
            // pcl::PointCloud<pcl::PointXYZ> dsp_cloud;
            pcl::PointCloud<pcl::PointXYZ>::Ptr laser_cloud(new pcl::PointCloud<pcl::PointXYZ>);
          //  pcl::PointCloud<pcl::PointXYZ>::Ptr dsp_cloud(new pcl::PointCloud<pcl::PointXYZ>);
            pcl::fromROSMsg(*msg, *laser_cloud);
            
            if (laser_cloud->empty()) {
                    RCLCPP_WARN(this->get_logger(), "Received empty point cloud. Skipping ICP.");
                    return;
            }
            std::size_t size;
            size= laser_cloud->points.size();
            RCLCPP_INFO(this->get_logger(), "Received %d point cloud.count=%d ",size,count);
            //保存文件
          //   if(count>50)return;
              
            //  if(count==20) { 
            //     std::string filename = "saved_pointcloud.pcd";
            //     pcl::io::savePCDFileASCII(filename, *dsp_cloud);
              
            //         RCLCPP_INFO(this->get_logger(), "Point cloud saved to %s", filename.c_str());
            //  }     ()
            // if(count==30)
            // {
            //     float x1,y1;
            //     filter_point(dsp_cloud);
                
                    
            //     comput3Dcenter(dsp_cloud,x1,y1);
            //     RCLCPP_INFO(this->get_logger(), "shift"); 
            //     shift_point(target_cloud,x1-x0,y1-y0);
            //     RCLCPP_INFO(this->get_logger(), "shift x=%f  y=%f ",x1-x0,y1-y0);
            //     ndt_test(target_cloud,dsp_cloud);
               
            // }      
             cut_cloud(laser_cloud,dsp_cloud);
            count++;
              // 转换回ROS消息 
            sensor_msgs::msg::PointCloud2 output_msg;
            pcl::toROSMsg(*dsp_cloud, output_msg);
             // 关键：复制原始消息的header 
            output_msg.header  = msg->header; // 同步frame_id和时间戳
            output_msg.header.stamp  = this->now(); // 或保持原时间戳
            pub_cloud->publish(output_msg);
            size= dsp_cloud->points.size();
            RCLCPP_INFO(this->get_logger(), "send %d point cloud. ",size);
        }
        rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr cloud_sub;
        rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub_cloud;
        sensor_msgs::msg::PointCloud2 output_msg;
        pcl::PointCloud<pcl::PointXYZ>::Ptr dsp_cloud;//(new pcl::PointCloud<pcl::PointXYZ>);
        pcl::PointCloud<pcl::PointXYZ>::Ptr target_cloud;
        int count;
        float x0,y0;
    };

    int main(int argc, char** argv) {
        rclcpp::init(argc, argv);
        auto node = std::make_shared<mult_lidar>();
        rclcpp::spin(node);
        rclcpp::shutdown();
        return 0;
    }